// This program demonstrates how to use the built-in features of the minibot and its software library.

#include <Servo.h>  // we need this library because we are using a servo
#include <Protobot.h>  // the Protobot library contains the functions made specifically for this minibot.

Protobot robot;  // create a Protobot object se we can use its functions
byte currentAngle; //Stores the angle the servo is pointed at
byte servoTurnDirection; //The direction the servo should be turning(+:right, -:left) and the step increment(must be divisible by 160 and 20
byte distanceLimit = 30; //The distance beyond which the program treats detections as background
int objectDistance = 0;
byte approachDistance = 10;
byte leftTurnLimit = 20;
byte rightTurnLimit = 160;

void setup()  // this runs once after reset
{
  robot.begin();  // sets up the hardware connections
  Serial.begin(9600);  // start USB serial interface 
  delay(3000); // wait 3 seconds
  Serial.println("hello, world!"); // say hello to anyone who is listening
}

byte search(byte currentAngle, byte servoTurnDirection) //given the current angle and the direction to turn returns a new current angle and looks for an object
{
  Serial.println(currentAngle);
  Serial.println(servoTurnDirection);
  Serial.println(robot.readSensor());
  currentAngle = currentAngle + servoTurnDirection;
  robot.turnServo(currentAngle);
  delay(100);
  if (robot.readSensor() < distanceLimit) 
  {  
    objectDistance = robot.readSensor();
    return currentAngle;
  }
  else
  {
    if (currentAngle == leftTurnLimit || currentAngle == rightTurnLimit) 
    servoTurnDirection = servoTurnDirection * -1;
  }
  currentAngle = search(currentAngle, servoTurnDirection);
  return currentAngle;
}

void pointAt(byte currentAngle)
{
  robot.turnServo(90);
  delay(500);
  if (currentAngle > 90)
  {
    robot.turnRight(50);
    while(robot.readSensor() > distanceLimit);
    robot.stop();
    return;
  }
  else if (currentAngle == 90)
  {
    return;
  }
  else
  {
    robot.turnLeft(50);
    while(robot.readSensor() > distanceLimit);
    robot.stop();
    return;
  }
  
}
void loop() {
  // put your main code here, to run repeatedly: 
  currentAngle = 20;
  servoTurnDirection = 5;
  currentAngle = search(currentAngle, servoTurnDirection);
  pointAt(currentAngle);
  robot.forward(100);
  
  while (robot.readSensor() > approachDistance)
  {
    if (robot.readSensor() >= objectDistance + 2)
    {
      robot.stop();
      robot.turnServo(leftTurnLimit);
      servoTurnDirection = 5;
      delay(500);
      currentAngle = search(currentAngle, servoTurnDirection);
      pointAt(currentAngle);
      robot.forward(100);
    }
  }
  robot.stop();
  delay(5000);
  robot.reverse(150);
  delay(3000);
  robot.stop();
  delay(5000);
}
